function [P_mvdr, f] = mvdr(x, fs, p, NFFT)
% MVDR线谱估计函数
% 输入参数：
%   x     : 输入信号（一维向量）
%   fs    : 采样频率 (Hz)
%   p     : 自相关矩阵阶数（通常取信号成分数的2倍）
%   NFFT  : 频率点数（默认1024）
% 输出参数：
%   P_mvdr: MVDR功率谱估计值
%   f     : 对应的频率向量 (Hz)

if nargin < 4
    NFFT = 1024; % 默认频率点数
end

N = length(x);

% 1. 估计自相关矩阵 R
r = xcorr(x, p-1, 'biased'); % 计算自相关函数（有偏估计）
r = r(p:end); % 取非负延迟部分 (0 到 p-1 延迟)
R = toeplitz(r); % 构造托普利兹自相关矩阵

% 添加正则化项确保矩阵可逆（可选）
epsilon = 1e-6 * trace(R)/p;
R = R + epsilon * eye(p);

% 2. 计算频率网格
f = linspace(-fs/2, fs/2-1, NFFT);
w = 2*pi*f/fs; % 归一化角频率

% 3. 计算MVDR谱
P_mvdr = zeros(NFFT, 1);
for k = 1:NFFT
    a = exp(-1j * w(k) * (0:p-1)'); % 导向矢量
    P_mvdr(k) = 1 / (a' * (R \ a)); % P = 1/(a^H R^{-1} a)
end
P_mvdr = real(P_mvdr); % P_{mvdr} is a positive real number

end